/*
 * Sonar.java
 *
 *Autors:   Jonathan Hernandez (jonathanalejandro@gmail.com)
 *          Alejandro Pustowka (bodom_chil1@hotmail.com)
 *
 * Clase que implementa un anillo de ultrasonidos. 
 */

/**
 * <b> Descripci&oacute;n </b> <p align=justify>
 *  </p>
 *
 * @author Jonathan Hernandez (<I><a href="mailto:jonathanalejandro@gmail.com">jonathanalejandro@gmail.com</a></I>) 
 * @author <br>alejandro Pustowka (<I><a href="mailto:bodom_chil1@hotmail.com">bodom_chil1@hotmail.com</a></I>)</br>
 */

package Robot.Sensors;
import Robot.*;
import javaclient3.*;
import java.util.ArrayList;
import javaclient3.structures.PlayerConstants;
import javaclient3.structures.PlayerPose3d;
/**
 * Clase que implementa el manejo de los sensores ultrasonido
 */
public class Sonar extends Sensor implements FloatArraySensor {
    
    SonarInterface sonarInterface;
    int numSonars;
    float [] data;
    double [] poseAngles;
    boolean isGeomQueried = false;
    
    public Sonar(Robot robot, String name, int index){
        
        super(robot, name, index);

        try
        {
        sonarInterface=robot.requestInterfaceSonar(deviceIndex, PlayerConstants.PLAYER_OPEN_MODE);
        }
        catch(Exception e)
        {
            e.printStackTrace();
        }
        if(sonarInterface!=null)
            System.out.println("interface sonar "+Integer.toString(deviceIndex)+" Ok");
        else{
            System.out.println("interface sonar "+Integer.toString(deviceIndex)+" Fail");
            return;
        }
        
            this.setPlayerDevice(sonarInterface);
            
    }

    public float[] read(int mode) {
        
        //if READ_MODE_WAIT it will waits until new data is available
        //else, returns the old data
        switch(mode)
        {
            case READ_MODE_WAIT:
                while(!update());
            break;
            case READ_MODE_NO_WAIT:
                update();
            break;
        
        }
        return data; 
    }
    
    public boolean update(){
        
        if(isReady()) {
            float[] temp = sonarInterface.getData().getRanges();

            for(int i=0;i<numSonars;i++){

                data[i]=temp[i];

                
            }
            return true;
        }
        else
            return false;
    }
    
    public boolean updatePoses(){
        
        
        
        if(!isGeomQueried){
            sonarInterface.queryGeometry();
            isGeomQueried = true;
        }
        
        PlayerPose3d [] posesTemp;
            
        

        //wait until geometry data is obtained from server
        if(sonarInterface.isGeomReady())
        {

            numSonars=sonarInterface.getGeom().getPoses_count();
            posesTemp=sonarInterface.getGeom().getPoses();
            

            System.out.println("Number of Poses for sonar "+Integer.toString(deviceIndex)+": "+ Integer.toString(numSonars));

            poseAngles = new double[numSonars];

            data = new float[numSonars];


            //get angle of poses
            for(int i=0;i<numSonars;i++){
            
                poseAngles[i]=posesTemp[i].getPyaw();

                System.out.println("x: "+Double.toString(posesTemp[i].getPx())+" y: "+Double.toString(posesTemp[i].getPy())+" yaw: "+Double.toString(Math.toDegrees(posesTemp[i].getPyaw())));
            }

            
            return true;
        }
        else
        {
            return false;
        }
    
    }
    
    
    public double [] getPoses(){
        return poseAngles;
    }
    
    public boolean isReady()
    {
        return sonarInterface.isDataReady();
    }
}